using System;
using System.Collections.Generic;
namespace OpenCvSharp
{
    public static class CvOperator
    {
        public static double ToRad(double deg)
        {
            return Cv2.PI / 180d * deg;
        }

        public static double ToDeg(double rad)
        {
            return 180d / Cv2.PI * rad;
        }

        public static double LineOrientation(double beginX, double beginY, double endX, double endY)
        {
            double angle = ToDeg(Math.Atan2(endY - beginY, endX - beginX));
            if (angle < 0D)
            {
                angle += 360D;
            }
            return angle;
        }
        public static double LineOrientation(this CvLine line)
        {
            return LineOrientation(line.BeginX, line.BeginY, line.EndX, line.EndY);
        }

        public static void LinePosition(double beginX, double beginY, double endX, double endY, out double centerX, out double centerY, out double angle)
        {
            centerX = (beginX + endX) / 2D;
            centerY = (beginY + endY) / 2D;
            angle =  LineOrientation(beginX, beginY, endX, endY);
        }
        public static void LinePosition(this CvLine line, out CvPoint center, out double angle)
        {
            double centerX, centerY;
            LinePosition(line.BeginX, line.BeginY, line.EndX, line.EndY, out centerX, out centerY, out angle);
            center = new CvPoint(centerX, centerY);
        }

        public static void MovePoint(double x, double y, double angle, double distance, out double nx, out double ny)
        {
            double rad = ToRad(angle);
            double sa = Math.Sin(rad);
            double ca = Math.Cos(rad);
            nx = x + distance * ca;
            ny = y + distance * sa;
        }
        public static CvPoint MovePoint(CvPoint point, double angle, double distance)
        {
            double nx, ny;
            MovePoint(point.X, point.Y, angle, distance, out nx, out ny);
            return new CvPoint(nx, ny);
        }

        public static void PointRotate(double beginX, double beginY, double centerX, double centerY, double angle, out double endX, out double endY)
        {
            double rad = ToRad(angle);
            double sa = Math.Sin(rad);
            double ca = Math.Cos(rad);
            endX = (beginX - centerX) * ca - (beginY - centerY) * sa + centerX;
            endY = (beginY - centerY) * ca + (beginX - centerX) * sa + centerY;
        }
        public static CvPoint PointRotate(this CvPoint point, CvPoint center, double angle)
        {
            double endX, endY;
            PointRotate(point.X, point.Y, center.X, center.Y, angle, out endX, out endY);
            return new CvPoint(endX, endY);
        }

        public static double DistancePP(double x1, double y1, double x2, double y2)
        {
            return Math.Sqrt(Math.Pow(x1 - x2, 2D) + Math.Pow(y1 - y2, 2D));
        }
        public static double DistancePP(this CvPoint point1, CvPoint point2)
        {
            return DistancePP(point1.X, point1.Y, point2.X, point2.Y);
        }

        public static double DistancePl(double x, double y, double x1, double y1, double x2, double y2)
        {
            if (x1 == x2)
            {
                return Math.Abs(x - x1);
            }
            double k = (y2 - y1) / (x2 - x1);
            double c = (x2 * y1 - x1 * y2) / (x2 - x1);
            double val = Math.Abs(k * x - y + c) / Math.Sqrt(k * k + 1D);
            return val;
        }
        public static double DistancePl(this CvPoint point, CvLine line)
        {
            return DistancePl(point.X, point.Y, line.BeginX, line.BeginY, line.EndX, line.EndY);
        }
        public static double DistancePl(this CvLine line, CvPoint point)
        {
            return DistancePl(point.X, point.Y, line.BeginX, line.BeginY, line.EndX, line.EndY);
        }

        public static void CalcRotatedRectPoints(double x, double y, double angle, double width, double height, out double[] xs, out double[] ys)
        {
            double rad = ToRad(angle);
            double sa = Math.Sin(rad) / 2D;
            double ca = Math.Cos(rad) / 2D;
            xs = new double[8];
            ys = new double[8];

            xs[0] = x - sa * height - ca * width;
            ys[0] = y + ca * height - sa * width;
            xs[1] = x + sa * height - ca * width;
            ys[1] = y - ca * height - sa * width;
            xs[2] = x * 2D - xs[0];
            ys[2] = y * 2D - ys[0];
            xs[3] = x * 2D - xs[1];
            ys[3] = y * 2D - ys[1];

            xs[4] = (xs[0] + xs[1]) / 2D;
            ys[4] = (ys[0] + ys[1]) / 2D;
            xs[5] = (xs[1] + xs[2]) / 2D;
            ys[5] = (ys[1] + ys[2]) / 2D;
            xs[6] = (xs[2] + xs[3]) / 2D;
            ys[6] = (ys[2] + ys[3]) / 2D;
            xs[7] = (xs[3] + xs[0]) / 2D;
            ys[7] = (ys[3] + ys[0]) / 2D;
        }
        public static CvPoint[] CalcRotatedRectPoints(double x, double y, double angle, double width, double height)
        {
            double[] xs, ys;
            CalcRotatedRectPoints(x, y, angle, width, height, out xs, out ys);
            CvPoint[] ps = new CvPoint[xs.Length];
            for (int i = 0; i < ps.Length; i++)
            {
                ps[i] = new CvPoint(xs[i], ys[i]);
            }
            return ps;
        }
        public static CvPoint[] CalcRotatedRectPoints(this CvRotatedRect rotatedRect)
        {
            return CalcRotatedRectPoints(rotatedRect.CenterX, rotatedRect.CenterY, rotatedRect.Angle, rotatedRect.Width, rotatedRect.Height);
        }

        public static CvCircle FitCircle(List<CvPoint> points)
        {
            CvCircle result = new CvCircle(0f, 0f, 0f);

            double X1 = 0d, Y1 = 0d, X2 = 0d, Y2 = 0d, X3 = 0d, Y3 = 0d, X1Y1 = 0d, X1Y2 = 0d, X2Y1 = 0d;

            for (int i = 0; i < points.Count; i++)
            {
                X1 += points[i].X;
                Y1 += points[i].Y;
                X2 += Math.Pow(points[i].X, 2d);
                Y2 += Math.Pow(points[i].Y, 2d);
                X3 += Math.Pow(points[i].X, 3d);
                Y3 += Math.Pow(points[i].Y, 3d);
                X1Y1 += points[i].X * points[i].Y;
                X1Y2 += points[i].X * Math.Pow(points[i].Y, 2d);
                X2Y1 += Math.Pow(points[i].X, 2d) * points[i].Y;
            }
            double C, D, E, G, H, N;
            double a, b, c;
            N = points.Count;
            C = N * X2 - X1 * X1;
            D = N * X1Y1 - X1 * Y1;
            E = N * X3 + N * X1Y2 - (X2 + Y2) * X1;
            G = N * Y2 - Y1 * Y1;
            H = N * X2Y1 + N * Y3 - (X2 + Y2) * Y1;
            a = (H * D - E * G) / (C * G - D * D);
            b = (H * C - E * D) / (D * D - G * C);
            c = -(a * X1 + b * Y1 + X2 + Y2) / N;
            result.CenterX = a / -2d;
            result.CenterY = b / -2d;
            result.Radius = Math.Sqrt(a * a + b * b - 4d * c) / 2d;
            return result;
        }
        //简单的矩阵转换实现的标定
        public static void Calibrate1(IEnumerable<CvPoint> robotPoints, IEnumerable<CvPoint> cameraPoints, Dictionary<string, double> calibResult)
        {
            calibResult = new Dictionary<string, double>();
            using (Mat homMat2d = Cv2.EstimateAffinePartial2D(InputArray.Create(cameraPoints,MatType.CV_64FC2), InputArray.Create(robotPoints, MatType.CV_64FC2), null,
                RobustEstimationAlgorithms.RANSAC, 3d, 2000ul, 0.99d))
            {
                calibResult.Add("a1", homMat2d.Get<double>(0, 0));
                calibResult.Add("b1", homMat2d.Get<double>(0, 1));
                calibResult.Add("c1", homMat2d.Get<double>(0, 2));
                calibResult.Add("a2", homMat2d.Get<double>(1, 0));
                calibResult.Add("b2", homMat2d.Get<double>(1, 1));
                calibResult.Add("c2", homMat2d.Get<double>(1, 2));
            }
            CvPoint tmp, max = new CvPoint(0d, 0d);
            foreach (CvPoint camP in cameraPoints)
            {
                Calibrate1Apply(calibResult, camP, out tmp);
                tmp.X -= camP.X;
                tmp.Y -= camP.Y;
                if (Math.Abs(tmp.X) > max.X)
                {
                    max.X = Math.Abs(tmp.X);
                }
                if (Math.Abs(tmp.Y) > max.Y)
                {
                    max.Y = Math.Abs(tmp.Y);
                }
            }
            calibResult.Add("maxX", max.X);
            calibResult.Add("maxY", max.Y);
        }
        public static void Calibrate1Apply(Dictionary<string, double> calibResult, CvPoint cameraPoint, out CvPoint robotPoint)
        {
            double a1 = calibResult["a1"] * cameraPoint.X;
            double b1 = calibResult["b1"] * cameraPoint.Y;
            double c1 = calibResult["c1"];
            double a2 = calibResult["a2"] * cameraPoint.X;
            double b2 = calibResult["b2"] * cameraPoint.Y;
            double c2 = calibResult["c2"];
            robotPoint = new CvPoint(a1 + b1 + c1, a2 + b2 + c2);
        }
    }
}
